#include "cem/material_utils.h"

#include <array>
#include <cmath>

namespace cem {
namespace material {

/*
函数名: build_rotation_matrix
用途: 构建主动旋转的 3x3 旋转矩阵 R, 用于将主轴张量 D 旋转到世界坐标系: T = R D R^T。
角度定义:
- strike_deg: 绕 Z 轴的角(度)，对应 legacy 代码中的 alphS；
- dip_deg: 绕 X 轴的角(度)，对应 legacy 代码中的 alphD；
- slant_deg: 再次绕 Z 轴的角(度)，对应 legacy 代码中的 alphL。
旋转顺序: Proper Euler ZXZ（主动旋转），即 R = Rz(strike) * Rx(dip) * Rz(slant)。
坐标系: 右手系，输入角度为度，函数内部转换为弧度。
说明: 该顺序与 legacy 中用于各向异性张量构造的闭式公式严格一致。
*/
std::array<std::array<double, 3>, 3>
build_rotation_matrix(double strike_deg, double dip_deg, double slant_deg)
{
    const double az = deg_to_rad(strike_deg);
    const double ax = deg_to_rad(dip_deg);
    const double ay = deg_to_rad(slant_deg);

    const double cz = std::cos(az), sz = std::sin(az);
    const double cx = std::cos(ax), sx = std::sin(ax);
    const double cy = std::cos(ay), sy = std::sin(ay);

    std::array<std::array<double, 3>, 3> Rz{{{cz, -sz, 0.0}, {sz, cz, 0.0}, {0.0, 0.0, 1.0}}};
    std::array<std::array<double, 3>, 3> Rx{{{1.0, 0.0, 0.0}, {0.0, cx, -sx}, {0.0, sx, cx}}};
    // slant_deg 对应再次绕 Z 轴旋转，构建第二个 Rz（使用 ay 的 cos/sin）
    std::array<std::array<double, 3>, 3> Rz2{{{cy, -sy, 0.0}, {sy, cy, 0.0}, {0.0, 0.0, 1.0}}};

    auto mul = [](const std::array<std::array<double, 3>, 3> & A,
                  const std::array<std::array<double, 3>, 3> & B)
    {
        std::array<std::array<double, 3>, 3> C{};
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
            {
                double s = 0.0;
                for (int k = 0; k < 3; ++k)
                    s += A[i][k] * B[k][j];
                C[i][j] = s;
            }
        return C;
    };

    return mul(mul(Rz, Rx), Rz2);
}

std::array<std::array<double, 3>, 3>
build_tensor_from_principal(double p1, double p2, double p3,
                            double strike_deg, double dip_deg, double slant_deg)
{
    const auto R = build_rotation_matrix(strike_deg, dip_deg, slant_deg);
    std::array<std::array<double, 3>, 3> RD{};
    const double D[3] = {p1, p2, p3};
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            RD[i][j] = R[i][j] * D[j];

    std::array<std::array<double, 3>, 3> T{};
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
        {
            double s = 0.0;
            for (int k = 0; k < 3; ++k)
                s += RD[i][k] * R[j][k]; // R^T[j][k] = R[k][j]
            T[i][j] = s;
        }
    return T;
}

std::array<std::array<double, 3>, 3>
build_inv_tensor_from_principal(double p1, double p2, double p3,
                                double strike_deg, double dip_deg, double slant_deg)
{
    auto safe_inv = [](double v)
    {
        const double eps = 1e-12;
        return 1.0 / ((std::fabs(v) < eps) ? (v >= 0 ? eps : -eps) : v);
    };
    return build_tensor_from_principal(
        safe_inv(p1), safe_inv(p2), safe_inv(p3), strike_deg, dip_deg, slant_deg);
}

double contract_gradients(const libMesh::RealGradient & gi,
                          const libMesh::RealGradient & gj,
                          const std::array<std::array<double, 3>, 3> & M,
                          unsigned int dim)
{
    double vi[3] = {0.0, 0.0, 0.0};
    double vj[3] = {0.0, 0.0, 0.0};
    for (unsigned int k = 0; k < dim && k < 3; ++k)
    {
        vi[k] = gi(k);
        vj[k] = gj(k);
    }
    double s = 0.0;
    for (int a = 0; a < 3; ++a)
    {
        double Ma_dot_vj = M[a][0] * vj[0] + M[a][1] * vj[1] + M[a][2] * vj[2];
        s += vi[a] * Ma_dot_vj;
    }
    return s;
}

double contract_vectors(const libMesh::RealVectorValue & vi,
                        const libMesh::RealVectorValue & vj,
                        const std::array<std::array<double, 3>, 3> & M,
                        unsigned int dim)
{
    double vvi[3] = {0.0, 0.0, 0.0};
    double vvj[3] = {0.0, 0.0, 0.0};
    for (unsigned int k = 0; k < dim && k < 3; ++k)
    {
        vvi[k] = vi(k);
        vvj[k] = vj(k);
    }
    double s = 0.0;
    for (int a = 0; a < 3; ++a)
    {
        double Ma_dot_vj = M[a][0] * vvj[0] + M[a][1] * vvj[1] + M[a][2] * vvj[2];
        s += vvi[a] * Ma_dot_vj;
    }
    return s;
}

} // namespace material
} // namespace cem